#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

nav_msgs::Odometry body;
void fast_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
   body = *msg;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fast_to_mavros");
  ros::NodeHandle nh;
  ros::Publisher odom_pub=nh.advertise<geometry_msgs::PoseStamped> ("/mavros/vision_pose/pose",10);
  ros::Subscriber fastlio_sub = nh.subscribe<nav_msgs::Odometry> ("/Odom_high_freq", 10, fast_cb);
  
  geometry_msgs::PoseStamped odom;
  setlocale(LC_ALL,"");
  ros::Rate rate(60.0);
  ROS_INFO("fast里程计融入飞控,60hz");
  while (ros::ok())
  {
    odom.header.stamp = ros::Time::now();
    //odom.header.frame_id = "base_link";
    odom.pose.position.x = body.pose.pose.position.x;
    odom.pose.position.y = body.pose.pose.position.y;
    odom.pose.position.z = body.pose.pose.position.z;
    odom.pose.orientation.x = body.pose.pose.orientation.x;
    odom.pose.orientation.y = body.pose.pose.orientation.y;
    odom.pose.orientation.z = body.pose.pose.orientation.z;
    odom.pose.orientation.w = body.pose.pose.orientation.w;
    
    odom_pub.publish(odom);
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}
